﻿// Inner Fire 游戏引擎库
// Collision - 碰撞检查
//
// Copyright (c) 2025 by 尔西大帝. All rights reserved.
//
// 所属层级：应用层（4）
// 用于检查碰撞。
//
// Author: 尔西大帝
// Email: 2670613871@qq.com
// Created: 2025-06-24

#include "collision.h"
#include <game/physics/bounding_capsule.h>
#include <game/physics/physics_math.h>
#include <graphics/imath.h>
#include <klog.h>

using namespace DirectX;

namespace ifire::game {

bool Collision::IsColliding(RigidBody* a, RigidBody* b, Contact& c) {
  if (a->GetShapeType() == ShapeType::Sphere) {
    if (b->GetShapeType() == ShapeType::Sphere) {
      return IsCollidingSphereSphere(a, b, c);
    } else if (b->GetShapeType() == ShapeType::Box) {
      return IsCollidingSphereBox(a, b, c);
    } else if (b->GetShapeType() == ShapeType::Capsule) {
      return IsCollidingSphereCapsule(a, b, c);
    }
  } else if (a->GetShapeType() == ShapeType::Box) {
    if (b->GetShapeType() == ShapeType::Sphere) {
      return IsCollidingSphereBox(b, a, c); // 注意这里反转了
    } else if (b->GetShapeType() == ShapeType::Box) {
      return IsCollidingBoxBox(a, b, c);
    } else if (b->GetShapeType() == ShapeType::Capsule) {
      return IsCollidingBoxCapsule(a, b, c);
    }
  } else if (a->GetShapeType() == ShapeType::Capsule) {
    if (b->GetShapeType() == ShapeType::Sphere) {
      return IsCollidingSphereCapsule(b, a, c); // 注意这里反转了
    } else if (b->GetShapeType() == ShapeType::Box) {
      return IsCollidingBoxCapsule(b, a, c); // 注意这里反转了
    } else if (b->GetShapeType() == ShapeType::Capsule) {
      return IsCollidingCapsuleCapsule(a, b, c);
    }
  }

  return false;
}

bool Collision::IsCollidingSphereSphere(
    RigidBody* a, RigidBody* b, Contact& c) {
  // 得到距离
  auto& a_sphere = a->GetBoundingSphere();
  auto& b_sphere = b->GetBoundingSphere();

  auto a_pos = XMLoadFloat3(&a_sphere.Center);
  auto b_pos = XMLoadFloat3(&b_sphere.Center);
  IVec distance = b_pos - a_pos;
  // 得到Radius的合
  auto radius_sum = a->collider_->sphere.radius + b->collider_->sphere.radius;

  // 如果半径的合大于距离，则碰撞发生
  bool is_colliding = imath::len_sq(distance) < (radius_sum * radius_sum);
  if (!is_colliding) {
    return false;
  }

  // 配置碰撞信息
  c.a = a;
  c.b = b;

  // 得到法线，就是从A到B的向量的归一化
  auto normal = XMVector3Normalize(distance);

  // Start其实就是从整个线段中从A出发到B，但减去掉B的半径。
  auto start = b_pos - normal * b->collider_->sphere.radius;
  // End其实就是从A出发，加上A的半径。
  auto end = a_pos + normal * a->collider_->sphere.radius;

  // 深度更加简单，因为已经有开始和结束向量，直接相减即可
  c.depth = imath::len(end - start);

  XMStoreFloat3(&c.start, start);
  XMStoreFloat3(&c.end, end);
  XMStoreFloat3(&c.normal, normal);

  return true;
}

static void ProjectOBB(
    const BoundingOrientedBox& box, XMVECTOR axis, float& min, float& max) {
  // 获取OBB的旋转矩阵和半长宽高
  XMMATRIX rotation =
      XMMatrixRotationQuaternion(XMLoadFloat4(&box.Orientation));
  XMVECTOR extents = XMLoadFloat3(&box.Extents);
  XMVECTOR center = XMLoadFloat3(&box.Center);

  // 计算旋转后的8个顶点
  XMVECTOR vertices[8];
  for (int i = 0; i < 8; ++i) {
    XMVECTOR sign = XMVectorSet((i & 1) ? 1.0f : -1.0f, (i & 2) ? 1.0f : -1.0f,
        (i & 4) ? 1.0f : -1.0f, 0.0f);
    vertices[i] = center + XMVector3Transform(extents * sign, rotation);
  }

  // 初始化投影范围
  min = max = XMVectorGetX(XMVector3Dot(vertices[0], axis));

  // 计算所有顶点在轴上的投影
  for (int i = 1; i < 8; ++i) {
    float projection = XMVectorGetX(XMVector3Dot(vertices[i], axis));
    min = (std::min)(min, projection);
    max = (std::max)(max, projection);
  }
}

static bool SATTest(const BoundingOrientedBox& a, const BoundingOrientedBox& b,
    float& depth, XMVECTOR& normal) {
  // 获取OBB的局部轴
  XMVECTOR a_axes[3], b_axes[3];
  XMMATRIX a_rot = XMMatrixRotationQuaternion(XMLoadFloat4(&a.Orientation));
  XMMATRIX b_rot = XMMatrixRotationQuaternion(XMLoadFloat4(&b.Orientation));
  a_axes[0] = a_rot.r[0]; // X轴
  a_axes[1] = a_rot.r[1]; // Y轴
  a_axes[2] = a_rot.r[2]; // Z轴
  b_axes[0] = b_rot.r[0];
  b_axes[1] = b_rot.r[1];
  b_axes[2] = b_rot.r[2];

  // 测试所有15个分离轴
  depth = FLT_MAX;
  for (int i = 0; i < 15; ++i) {
    XMVECTOR axis;
    if (i < 3)
      axis = a_axes[i]; // A的3个轴
    else if (i < 6)
      axis = b_axes[i - 3]; // B的3个轴
    else {                  // 9个边叉积轴
      int a_idx = (i - 6) / 3;
      int b_idx = (i - 6) % 3;
      axis = XMVector3Cross(a_axes[a_idx], b_axes[b_idx]);
      if (XMVector3Equal(axis, XMVectorZero())) continue; // 忽略平行边
      axis = XMVector3Normalize(axis);
    }

    // 计算投影重叠
    float a_min, a_max, b_min, b_max;
    ProjectOBB(a, axis, a_min, a_max);
    ProjectOBB(b, axis, b_min, b_max);
    float overlap = (std::min)(a_max, b_max) - (std::max)(a_min, b_min);

    if (overlap <= 0) return false; // 存在分离轴
    if (overlap < depth) {
      depth = overlap;
      normal = axis;
    }
  }

  // 确保法线从A指向B
  XMVECTOR a_to_b = XMLoadFloat3(&b.Center) - XMLoadFloat3(&a.Center);
  if (XMVectorGetX(XMVector3Dot(normal, a_to_b)) < 0) {
    normal = -normal;
  }

  return true;
}

bool Collision::IsCollidingBoxBox(RigidBody* a, RigidBody* b, Contact& c) {
  // 获取两个OBB
  auto& a_box = a->GetBoundingOrientedBox();
  auto& b_box = b->GetBoundingOrientedBox();

  // 1. 快速排斥测试
  if (!a_box.Intersects(b_box)) {
    return false;
  }

  // 2. 精确SAT检测
  float depth;
  XMVECTOR normal;
  if (!SATTest(a_box, b_box, depth, normal)) {
    return false;
  }
  normal = XMVector3Normalize(normal);

  // 4. 计算接触点（使用中心连线与表面的交点）
  XMVECTOR a_center = XMLoadFloat3(&a_box.Center);
  XMVECTOR b_center = XMLoadFloat3(&b_box.Center);
  XMVECTOR dir = b_center - a_center;
  XMVECTOR proj_a = a_center + normal * XMVectorGetX(XMVector3Dot(dir, normal));
  auto start = proj_a - normal * depth * 0.5f;
  auto end = proj_a + normal * depth * 0.5f;

  // 填充Contact信息
  c.a = a;
  c.b = b;
  c.depth = depth;
  XMStoreFloat3(&c.start, start);
  XMStoreFloat3(&c.end, end);
  XMStoreFloat3(&c.normal, normal);

  return true;
}

bool Collision::IsCollidingSphereBox(RigidBody* a, RigidBody* b, Contact& c) {
  // 1. 获取几何数据
  auto& a_sphere = a->GetBoundingSphere();
  auto& b_box = b->GetBoundingOrientedBox();

  // 2. 快速排斥测试
  if (!a_sphere.Intersects(b_box)) {
    return false;
  }

  // 3. 转换到OBB局部空间
  XMVECTOR sphere_center = XMLoadFloat3(&a_sphere.Center);
  XMVECTOR box_center = XMLoadFloat3(&b_box.Center);
  XMMATRIX box_inverse_rot = XMMatrixRotationQuaternion(
      XMQuaternionInverse(XMLoadFloat4(&b_box.Orientation)));

  XMVECTOR local_sphere_center =
      XMVector3Transform(sphere_center - box_center, box_inverse_rot);

  // 4. 计算OBB局部空间最近点
  XMVECTOR box_extents = XMLoadFloat3(&b_box.Extents);
  XMVECTOR closest_local =
      XMVectorClamp(local_sphere_center, -box_extents, box_extents);

  // 5. 转换回世界空间
  XMMATRIX box_rot =
      XMMatrixRotationQuaternion(XMLoadFloat4(&b_box.Orientation));
  XMVECTOR closest_point =
      XMVector3Transform(closest_local, box_rot) + box_center;

  // 6. 计算球体表面接触点
  XMVECTOR sphere_to_box = closest_point - sphere_center;
  float distance_sq = XMVectorGetX(XMVector3LengthSq(sphere_to_box));
  float radius_sq = a_sphere.Radius * a_sphere.Radius;

  if (distance_sq > radius_sq) {
    return false;
  }

  // 7. 填充Contact信息
  c.a = a;
  c.b = b;
  c.depth = a_sphere.Radius - sqrtf(distance_sq);
  auto normal = XMVector3Normalize(sphere_to_box); // 统一法线方向

  // 8. 计算接触点（确保非零冲量臂）
  auto start = closest_point;                          // 盒子接触点
  auto end = sphere_center + normal * a_sphere.Radius; // 球体表面点

  XMStoreFloat3(&c.start, start);
  XMStoreFloat3(&c.end, end);
  XMStoreFloat3(&c.normal, normal);
  return true;
}

bool Collision::IsCollidingSphereCapsule(
    RigidBody* sphere, RigidBody* capsule, Contact& c) {
  // 1. 获取世界空间几何数据
  auto& sph = sphere->GetBoundingSphere();
  auto& cap = capsule->GetBoundingCapsule();

  // 2. 计算胶囊体线段最近点
  XMVECTOR seg_start = XMLoadFloat3(&cap.start);
  XMVECTOR seg_end = XMLoadFloat3(&cap.end);
  XMVECTOR sphere_center = XMLoadFloat3(&sph.Center);

  // 线段方向处理（处理退化情况）
  XMVECTOR seg_vec = seg_end - seg_start;
  float seg_length = XMVectorGetX(XMVector3Length(seg_vec));
  XMVECTOR seg_dir =
      (seg_length > 1e-6f) ? seg_vec / seg_length : XMVectorSet(0, 1, 0, 0);

  // 3. 计算球心到线段的最近点
  XMVECTOR proj = XMVector3Dot(sphere_center - seg_start, seg_dir);
  proj = XMVectorClamp(proj, XMVectorZero(), XMVectorReplicate(seg_length));
  XMVECTOR closest = seg_start + proj * seg_dir;

  // 4. 计算真实法线方向（从胶囊体指向球体）
  XMVECTOR delta = sphere_center - closest;
  float dist = XMVectorGetX(XMVector3Length(delta));
  XMVECTOR normal =
      (dist > 1e-6f) ? delta / dist : seg_dir; // 完全重合时使用线段方向

  // 5. 精确计算接触点（物理正确方式）
  XMVECTOR capsule_surface_point =
      closest - normal * cap.radius; // 胶囊体表面点
  XMVECTOR sphere_surface_point =
      sphere_center + normal * sph.Radius; // 球体表面点

  // 6. 穿透深度计算（带方向）
  float penetration = (cap.radius + sph.Radius) - dist;
  if (penetration <= 0) return false;

  // 7. 填充接触信息（严格保证方向）
  c.a = sphere;
  c.b = capsule;
  c.depth = penetration;
  XMStoreFloat3(&c.normal, -normal);
  XMStoreFloat3(&c.start, capsule_surface_point);
  XMStoreFloat3(&c.end, sphere_surface_point);

  return true;
}

bool Collision::IsCollidingBoxCapsule(
    RigidBody* box, RigidBody* capsule, Contact& c) {
  // 1. 获取几何数据
  auto& obb = box->GetBoundingOrientedBox();
  auto& cap = capsule->GetBoundingCapsule();

  // 2. 转换胶囊体线段到OBB局部空间
  XMMATRIX box_inv_rot = XMMatrixRotationQuaternion(
      XMQuaternionInverse(XMLoadFloat4(&obb.Orientation)));
  XMVECTOR cap_start = XMLoadFloat3(&cap.start) - XMLoadFloat3(&obb.Center);
  XMVECTOR cap_end = XMLoadFloat3(&cap.end) - XMLoadFloat3(&obb.Center);
  cap_start = XMVector3Transform(cap_start, box_inv_rot);
  cap_end = XMVector3Transform(cap_end, box_inv_rot);

  // 3. 计算线段到AABB的最近点（局部空间）
  XMVECTOR box_min = -XMLoadFloat3(&obb.Extents);
  XMVECTOR box_max = XMLoadFloat3(&obb.Extents);
  XMVECTOR closest_on_box_local = physics_math::ClosestPointOnSegmentToAABB(
      cap_start, cap_end, box_min, box_max);

  // 4. 转换回世界空间
  XMMATRIX box_rot = XMMatrixRotationQuaternion(XMLoadFloat4(&obb.Orientation));
  XMVECTOR closest_on_box = XMVector3Transform(closest_on_box_local, box_rot) +
                            XMLoadFloat3(&obb.Center);

  // 5. 计算胶囊体表面最近点（世界空间）
  XMVECTOR seg_vec = XMLoadFloat3(&cap.end) - XMLoadFloat3(&cap.start);
  float seg_length = XMVectorGetX(XMVector3Length(seg_vec));
  XMVECTOR seg_dir =
      (seg_length > 1e-6f) ? seg_vec / seg_length : XMVectorSet(0, 1, 0, 0);
  XMVECTOR proj =
      XMVector3Dot(closest_on_box - XMLoadFloat3(&cap.start), seg_dir);
  proj = XMVectorClamp(proj, XMVectorZero(), XMVectorReplicate(seg_length));
  XMVECTOR closest_on_cap = XMLoadFloat3(&cap.start) + proj * seg_dir;

  // 6. 计算真实法线方向（从胶囊体指向盒子）
  XMVECTOR delta = closest_on_box - closest_on_cap;
  float dist = XMVectorGetX(XMVector3Length(delta));
  XMVECTOR normal = (dist > 1e-6f) ? delta / dist : XMVectorSet(0, 1, 0, 0);

  // 7. 计算盒子在法线方向的投影深度
  XMVECTOR local_normal = XMVector3TransformNormal(normal, box_inv_rot);
  float box_projection =
      XMVectorGetX(XMVector3Dot(box_max, XMVectorAbs(local_normal)));

  // 8. 穿透深度计算（盒子表面到胶囊体表面）
  float penetration = (cap.radius + box_projection) - dist;
  if (penetration <= 0) return false;

  // 9. 精确计算接触点
  XMVECTOR box_contact = closest_on_box;
  XMVECTOR capsule_contact = closest_on_cap + normal * cap.radius;

  // 10. 填充接触信息（法线方向从胶囊体指向盒子）
  c.a = box;
  c.b = capsule;
  c.depth = penetration;
  XMStoreFloat3(&c.normal, -normal);
  XMStoreFloat3(&c.start, capsule_contact); // 胶囊体表面点
  XMStoreFloat3(&c.end, box_contact);       // 盒子表面点

  return true;
}

bool Collision::IsCollidingCapsuleCapsule(
    RigidBody* a, RigidBody* b, Contact& c) {
  // 1. 获取几何数据
  auto& capA = a->GetBoundingCapsule();
  auto& capB = b->GetBoundingCapsule();

  // 2. 计算线段最近点对
  XMVECTOR a_start = XMLoadFloat3(&capA.start);
  XMVECTOR a_end = XMLoadFloat3(&capA.end);
  XMVECTOR b_start = XMLoadFloat3(&capB.start);
  XMVECTOR b_end = XMLoadFloat3(&capB.end);

  XMVECTOR closestA, closestB;
  physics_math::ClosestPointBetweenSegments(
      a_start, a_end, b_start, b_end, closestA, closestB);

  // 3. 计算真实法线方向（从A指向B）
  XMVECTOR delta = closestB - closestA;
  float dist = XMVectorGetX(XMVector3Length(delta));
  XMVECTOR normal = (dist > 1e-6f)
                        ? delta / dist
                        : XMVectorSet(0, 1, 0, 0); // 退化情况使用默认法线

  // 4. 穿透深度计算
  float total_radius = capA.radius + capB.radius;
  float penetration = total_radius - dist;
  if (penetration <= 0) return false;

  // 5. 精确计算接触点（考虑表面曲率）
  XMVECTOR contactA = closestA + normal * capA.radius;
  XMVECTOR contactB = closestB - normal * capB.radius;

  // 6. 填充接触信息（确保法线从A指向B）
  c.a = a;
  c.b = b;
  c.depth = penetration;
  XMStoreFloat3(&c.normal, normal);
  XMStoreFloat3(&c.start, contactA);
  XMStoreFloat3(&c.end, contactB);

  return true;
}

} // namespace ifire::game